{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/ostream.h>

// Import all the known types.
#include <sym/ops/lie_group_ops.h>
#include <sym/ops/storage_ops.h>
{% for cls in sf.GEO_TYPES + sf.CAM_TYPES %}
#include <sym/{{ python_util.camelcase_to_snakecase(cls.__name__) }}.h>
{% endfor %}
#include <sym/util/typedefs.h>
#include <symforce/opt/assert.h>

#include <lcmtypes/sym/type_t.hpp>

namespace sym {

template <typename T>
static constexpr const bool kIsEigenType = std::is_base_of<Eigen::MatrixBase<T>, T>::value;

template <typename T>
static constexpr const bool kIsSparseEigenType =
    std::is_base_of<Eigen::SparseMatrix<typename T::Scalar>, T>::value;

inline bool IsEigenType(const type_t type) {
  switch (type.value) {
    {% for i in range(1, 10) %}
    case type_t::VECTOR{{ i }}:
    {% endfor %}
    case type_t::VECTORX:
    {% for i in range(1, 10) %}
    {% for j in range(2, 10) %}
    case type_t::MATRIX{{ i }}{{ j }}:
    {% endfor %}
    {% endfor %}
      return true;
    {% for cls in sf.GEO_TYPES %}
      case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}:
    {% endfor %}
    case type_t::SCALAR:
    {% for cls in sf.CAM_TYPES %}
    case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}:
    {% endfor %}
    case type_t::DATABUFFER:
    case type_t::PREINTEGRATED_IMU_MEASUREMENTS:
      return false;
    case type_t::INVALID:
      SYM_ASSERT(false, "Invalid type");
  }
  SYM_ASSERT(false, "Unreachable");  // gcc (at least >=5,<=11) does not prove this is unreachable
}

/**
 *  Returns the shape of an Eigen type as a pair of ints (rows, cols)
 */
inline std::pair<int, int> EigenTypeShape(const type_t type) {
  switch (type.value) {
    {% for i in range(1, 10) %}
    case type_t::VECTOR{{ i }}:
      return { {{ i }}, 1 };
    {% endfor %}
    {% for i in range(1, 10) %}
    {% for j in range(2, 10) %}
    case type_t::MATRIX{{ i }}{{ j }}:
      return { {{ i }}, {{ j }} };
    {% endfor %}
    {% endfor %}
    default:
      SYM_ASSERT(false, "Invalid type for EigenTypeShape: {}", type);
  }
}

/**
 * Helper to handle polymorphism by creating a switch from a runtime type enum to dispatch
 * to the templated method func. Used to perform type-aware operations.
 *
 * Args:
 *   name: Name of the output function (ex: FormatByType)
 *   func: Name of a function template (ex: FormatHelper)
 */
#define BY_TYPE_HELPER(name, func, matrix_func)                        \
  template <typename Scalar, typename... Args>                         \
  auto name(const type_t type, Args&&... args) {                       \
    if (IsEigenType(type)) {                                           \
      return matrix_func<Scalar>(args...);                             \
    }                                                                  \
    switch (type.value) {                                              \
      {% for cls in sf.GEO_TYPES %}
      case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}: \
        return func<sym::{{ cls.__name__ }}<Scalar>>(args...);           \
      {% endfor %}
      case type_t::SCALAR:                                             \
        return func<Scalar>(args...);                                  \
      {% for cls in sf.CAM_TYPES %}
      case type_t::{{ python_util.camelcase_to_screaming_snakecase(cls.__name__) }}: \
        return func<sym::{{ cls.__name__ }}<Scalar>>(args...);         \
      {% endfor %}
      default:                                                         \
        SYM_ASSERT(false, "Unhandled type: {}", type);                 \
    }                                                                  \
  }

}  // namespace sym
